/* Includes ------------------------------------------------------------------*/
#include "stm32f10x.h"
#include "definiciones_hs_car.h"
#include "stm32f10x_it.h"
#include "histograma.h"
#include <stdio.h>

/* Private typedef -----------------------------------------------------------*/

/* Private define ------------------------------------------------------------*/

/* Private macro -------------------------------------------------------------*/

/* External variables --------------------------------------------------------*/

//Variables de recepcion
extern uint8_t USART1_pila_rx[260]; 
//Variables de envio
extern uint32_t USART1_pila_TX[260];
//Alarmas
extern uint32_t USART1_Byte_Timer;

//Estado de la maquina general de comunicaciones
extern uint8_t USART1_estado_general;

//Estado de la maquina de recepcion
extern uint8_t USART1_estado_rx;

//Resultado del PID de direccion, obtenido tras leer las barras y utilizado en control_vehiculo
extern float Resultado_PID_Direccion;

//Variables relacionadas con las entradas de radio (pwm)
uint32_t EntradaModoControl = PARADA_EMERGENCIA;
uint32_t EntradaDireccion = CENTRO_DIR;
uint32_t EntradaAceleracion = CENTRO_ACC;
uint32_t EntradaAux = CENTRO_AUX;

extern uint32_t PWM_IN_invalido;
extern uint32_t estado_PWM_IN;
extern uint32_t PWM_In_Secuencia[];
extern uint32_t PWM_In_Num_Medidas;
extern uint32_t Estado_Lectura_IRS;
extern uint32_t IRS_Por_Leer_PortB;
extern uint32_t IRS_Por_Leer_PortC;
extern uint32_t IRS_Por_Leer_PortD;
extern uint32_t IRS_Por_Leer_PortE;
extern uint32_t FLAG_lectura_ir_finalizada;
extern float ciclo_scan_ms;

extern float Ki_direccion;
extern float Kp_direccion;
extern float Kd_direccion;
extern float Error_Acumulado_direccion;
extern float Error_Anterior_direccion;

extern float Ki_gas;
extern float Kp_gas;
extern float Kd_gas;
extern float Error_Acumulado_gas;
extern float Error_Anterior_gas;

extern int64_t Pulsos_Encoder_Total_64;
extern float Velocidad_metros_s;
extern float Distancia_Total_metros;

int32_t Bias_Bifurcacion = 0;

//FLAGS para ejecutar funciones
extern uint32_t FLAG_Comienza_Lectura_Barras;

/* Private variables ---------------------------------------------------------*/
USART_InitTypeDef USART_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef  TIM_OCInitStructure;
TIM_ICInitTypeDef  TIM_ICInitStructure;
EXTI_InitTypeDef   EXTI_InitStructure;

uint32_t Luminosidad_Barra_Frontal[24];
uint32_t Luminosidad_Barra_Trasera[16];
float rango_dinamico_frontal[24];
float rango_dinamico_trasera[16];
uint32_t Luminosidad_Escalada_Frontal[24];
uint32_t Luminosidad_Escalada_Trasera[16];
float Calibraciones_Frontal_Oscuro[24];
float Calibraciones_Trasera_Oscuro[16];
float Calibraciones_Frontal_Claro[24];
float Calibraciones_Trasera_Claro[16];
float consigna_posicion = CONSIGNA_DIRECCION_CENTRO;
uint32_t Calibraciones_Frontal_OK[24];
uint32_t Calibraciones_Trasera_OK[16];
uint16_t Luminosidad_PortA[16];
uint16_t Luminosidad_PortB[16];
uint16_t Luminosidad_PortC[16];
uint16_t Luminosidad_PortD[16];
uint16_t Luminosidad_PortE[16];

uint32_t numero_puertas_atravesadas = 0;

uint32_t calibrando_barras=0;

uint32_t ms_restantes_beep = 0;

float Posicion_Linea=11.5;

/* Private function prototypes -----------------------------------------------*/
void NVIC_Configuration(void);
void USART_Configuration(void);
void GPIO_Configuration(void);
void Poll_IR(void);
void Almacena_Luminosidad_Barras(void);
void Comienza_Lectura_Barras(void);
void Envia_Telemetria(void);
void Configurar_Reloj(void);
void TIMER_Configuration(void);


//uint32_t vueltas_main_debug=0;
int main(void)
{
	int i;
	GPIO_InitTypeDef Estructura_GPIO_in_floating_50mhz;
	Estructura_GPIO_in_floating_50mhz.GPIO_Speed = GPIO_Speed_50MHz; 
  	Estructura_GPIO_in_floating_50mhz.GPIO_Mode = GPIO_Mode_IN_FLOATING;


  /*!< At this stage the microcontroller clock setting is already configured, 
       this is done through SystemInit() function which is called from startup
       file (startup_stm32f10x_xx.s) before to branch to application main.
       To reconfigure the default setting of SystemInit() function, refer to
       system_stm32f10x.c file
  */ 
  
	// Configuramos el reloj a 36Mhz (Interno/2 * 9) Es el maximo que se puede conseguir con el interno.
  Configurar_Reloj();

 
	if (DEBUG_CLOCK)
	{
		GPIO_InitTypeDef GPIO_InitStructure;
		//Sacamos por la pata PA8 el reloj del sistema para comprobar su correcto funcionamiento 
	  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); 
	  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
	  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
	  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; 
	  GPIO_Init(GPIOA, &GPIO_InitStructure); 

	  //RCC_MCOConfig(RCC_MCO_HSE); // Sacamos la frecuencia del cristal externo !! cristal externo no conectado. usamos esas patas para otra cosa
	  RCC_MCOConfig(RCC_MCO_SYSCLK);  // Sacamos la frecuencia del reloj del sistema por PA8 (MCO)

		while(1);	//bucle infinito para probar.
	}
	
  RCC_APB1PeriphClockCmd (RCC_APB1Periph_TIM5 | RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4, ENABLE);
  RCC_APB2PeriphClockCmd (RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD | RCC_APB2Periph_GPIOE | RCC_APB2Periph_USART1 | RCC_APB2Periph_AFIO | RCC_APB2Periph_TIM1, ENABLE); 
	RCC_AHBPeriphClockCmd  (RCC_AHBPeriph_CRC, ENABLE);
  USART_Configuration();
	TIMER_Configuration();
  GPIO_Configuration();
	NVIC_Configuration();  
	Estado_Lectura_IRS = IR_INACTIVO;

	for (i=0;i<24;i++)
	{ 
		Calibraciones_Frontal_Oscuro[i] = 0;
		Calibraciones_Frontal_Claro[i] = 0;
		Calibraciones_Frontal_OK[i] = 0;
	}
	for (i=0;i<16;i++)
	{
		Calibraciones_Trasera_Oscuro[i] = 0; 
		Calibraciones_Trasera_Claro[i] = 0; 
		Calibraciones_Trasera_OK[i] = 0; 
	}

	reiniciar_histograma();

	if (GPIO_ReadInputDataBit(Puerto_FC_Colision, Pin_FC_Colision)) 
	{
		calibrando_barras=1;
	}

	//Habilita la interrupcion del TIM1, utilizado para contar milisegundos
 	TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);
	
	//Habilita la interrupcion de recepcion de la UART1 (Salta la interrupcion cuando el registro de recepcion tiene algo)
  //USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);

		
  while (1)
  {
		//vueltas_main_debug ++;
		
		//Detectar un timeout de recepcion y actuar en consecuencia
		if ((USART1_Byte_Timer==0) && (USART1_estado_rx!=USART1_RX_ESTADO_CABECERA)) USART1_Reiniciar_Maquina_rx();


		if (FLAG_Comienza_Lectura_Barras) Comienza_Lectura_Barras();

		//if ((USART1->CR1 & 0x0080) == 0) Envia_Telemetria();

	 	//Polling de los sensores IR
		switch(Estado_Lectura_IRS )
		{
			case IR_LEYENDO_BARRAS:
				if (!FLAG_lectura_ir_finalizada)	Poll_IR();
				break;
			
			case IR_CARGANDO_BARRAS:
				if (TIM5->CNT > IR_TIEMPO_CARGA)
				{
					Estado_Lectura_IRS = IR_LEYENDO_BARRAS;

					IRS_Por_Leer_PortB = PINES_BARRAS_PORTB;
					IRS_Por_Leer_PortC = PINES_BARRAS_PORTC; 
					IRS_Por_Leer_PortD = PINES_BARRAS_PORTD;
					IRS_Por_Leer_PortE = PINES_BARRAS_PORTE;
					
					//Configuramos todos los pines asociados a las barras como entrada
				  Estructura_GPIO_in_floating_50mhz.GPIO_Pin = PINES_BARRAS_PORTB;
				  GPIO_Init(GPIOB, &Estructura_GPIO_in_floating_50mhz); 
		
				  Estructura_GPIO_in_floating_50mhz.GPIO_Pin = PINES_BARRAS_PORTC; 
				  GPIO_Init(GPIOC, &Estructura_GPIO_in_floating_50mhz); 
				
					Estructura_GPIO_in_floating_50mhz.GPIO_Pin = PINES_BARRAS_PORTD;
				  GPIO_Init(GPIOD, &Estructura_GPIO_in_floating_50mhz);
				
					Estructura_GPIO_in_floating_50mhz.GPIO_Pin = PINES_BARRAS_PORTE;
				  GPIO_Init(GPIOE, &Estructura_GPIO_in_floating_50mhz);
					
					TIM5->CNT = 0;					
				}
				break;
									
			default: // Caso inactivo.
				break;		
  	}
	}
}

void Configurar_Reloj(void) 
{ 
	ErrorStatus HSEStartUpStatus;
  /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------------------*/    
  /* RCC system reset(for debug purpose) */ 
  RCC_DeInit(); 
 
  /* Enable HSE */ 
  RCC_HSEConfig(RCC_HSE_ON); 
 
  /* Wait till HSE is ready */ 
  HSEStartUpStatus = RCC_WaitForHSEStartUp(); 
 
  if (HSEStartUpStatus == SUCCESS) 
  { 
    /* Flash 0 wait state */ 
    FLASH_SetLatency(FLASH_Latency_0); 
    /* HCLK = SYSCLK */ 
    RCC_HCLKConfig(RCC_SYSCLK_Div1);  
   
    /* PCLK2 = HCLK */ 
    RCC_PCLK2Config(RCC_HCLK_Div1);  
 
    /* PCLK1 = HCLK/2 */ 
    RCC_PCLK1Config(RCC_HCLK_Div1); //Ojo! No debe superar 36Mhz
     
    RCC_PLLConfig(RCC_PLLSource_HSI_Div2, RCC_PLLMul_9); //Usamos el interno puesto que las patas del xtal externo estan ocupadas.
 
    /* Enable PLL */  
    RCC_PLLCmd(ENABLE); 
 
    /* Wait till PLL is ready */ 
    while (RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET) 
    { 
    } 
 
    /* Select PLL as system clock source */ 
    RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK); 
 
    /* Wait till PLL is used as system clock source */ 
    while (RCC_GetSYSCLKSource() != 0x08) 
    { 
    } 
  } 
  else 
  { 
    /* If HSE fails to start-up, the application will have wrong clock configuration. 
       User can add here some code to deal with this error */     
 
    /* Go to infinite loop */ 
    while (1) 
    { 
    } 
  } 
}

//
uint32_t HayLinea(void)
{
	//Devuelve 1 si hay HAYLINEA_NUM o mas sensores que detecten 'linea'.
	//Se detecta 'linea' si el valor del sensor esta por debajo de HAYLINEA_THRES*RangoDinamico	+
	uint32_t i;
	uint32_t nlineas_ok=0;
	for (i=0;i<24;i++)
	{	
		if ( Luminosidad_Barra_Frontal[i] < (rango_dinamico_frontal[i]*HAYLINEA_THRES + Calibraciones_Frontal_Claro[i])) 
			nlineas_ok++;		
	}
//	for (i=0;i<16:i++)
//		if ( Luminosidad_Barra_Trasera[i] < rango_dinamico_trasera[i]*HAYLINEA_THRES + Calibraciones_Trasera_Claro[i]) nlineas_ok++;		
	if (nlineas_ok >= HAYLINEA_NUM) return 1;
	else return 0;

}

//Recorre los puertos y guarda valores del timer1 para los pines recien desactivados (Lectura IR)
void Poll_IR(void)
{
	uint32_t pinpos;
	uint32_t pos;
	uint16_t IR_PinesDeInteres;
	uint16_t IR_RecienDesactivados;
	uint32_t FLAG_Fuerza_Finalizacion_Lectura_IR;

	uint16_t Valor_Timer = TIM5->CNT;
	FLAG_Fuerza_Finalizacion_Lectura_IR = (Valor_Timer > IR_TIMEOUT*1000) ? 1:0;

 	if (IRS_Por_Leer_PortB != 0)
	{
		IR_PinesDeInteres = IRS_Por_Leer_PortB & GPIO_ReadInputData(GPIOB);
 		if (FLAG_Fuerza_Finalizacion_Lectura_IR)
			IR_RecienDesactivados = IRS_Por_Leer_PortB;
		else 
			IR_RecienDesactivados = IR_PinesDeInteres ^ IRS_Por_Leer_PortB;
		for (pinpos = 0; (pinpos<16); pinpos++)
	    {
	      pos = ((uint32_t)0x01) << pinpos;
				if (pos & IR_RecienDesactivados)
				{
					Luminosidad_PortB[pinpos] = Valor_Timer;
				}
			}
		IRS_Por_Leer_PortB = IRS_Por_Leer_PortB & (~IR_RecienDesactivados);
	}

 	if (IRS_Por_Leer_PortC != 0)
	{
		IR_PinesDeInteres = IRS_Por_Leer_PortC &  GPIO_ReadInputData(GPIOC);
		if (FLAG_Fuerza_Finalizacion_Lectura_IR)
			IR_RecienDesactivados = IRS_Por_Leer_PortC;
		else 
			IR_RecienDesactivados = IR_PinesDeInteres ^ IRS_Por_Leer_PortC;
		for (pinpos = 0; (pinpos<16); pinpos++)
	    {
	      pos = ((uint32_t)0x01) << pinpos;
				if (pos & IR_RecienDesactivados)
				{
					Luminosidad_PortC[pinpos] = Valor_Timer;
				}
			}
		IRS_Por_Leer_PortC = IRS_Por_Leer_PortC & (~IR_RecienDesactivados);
	}

 	if (IRS_Por_Leer_PortD != 0)
	{
		IR_PinesDeInteres = IRS_Por_Leer_PortD &  GPIO_ReadInputData(GPIOD);
		if (FLAG_Fuerza_Finalizacion_Lectura_IR)
			IR_RecienDesactivados = IRS_Por_Leer_PortD;
		else 
			IR_RecienDesactivados = IR_PinesDeInteres ^ IRS_Por_Leer_PortD;
		for (pinpos = 0; (pinpos<16); pinpos++)
	    {
	      pos = ((uint32_t)0x01) << pinpos;
				if (pos & IR_RecienDesactivados)
				{
					Luminosidad_PortD[pinpos] = Valor_Timer;
				}
			}
		IRS_Por_Leer_PortD = IRS_Por_Leer_PortD & (~IR_RecienDesactivados);
	}

 	if (IRS_Por_Leer_PortE != 0)
	{
		IR_PinesDeInteres = IRS_Por_Leer_PortE &  GPIO_ReadInputData(GPIOE);
		if (FLAG_Fuerza_Finalizacion_Lectura_IR)
			IR_RecienDesactivados = IRS_Por_Leer_PortE;
		else 
			IR_RecienDesactivados = IR_PinesDeInteres ^ IRS_Por_Leer_PortE;
		for (pinpos = 0; (pinpos<16); pinpos++)
	    {
	      pos = ((uint32_t)0x01) << pinpos;
				if (pos & IR_RecienDesactivados)
				{
					Luminosidad_PortE[pinpos] = Valor_Timer;
				}
			}
		IRS_Por_Leer_PortE = IRS_Por_Leer_PortE & (~IR_RecienDesactivados);
	}

	if (( 0x0000FFFF&(IRS_Por_Leer_PortB|IRS_Por_Leer_PortC|IRS_Por_Leer_PortD|IRS_Por_Leer_PortE))  == 0)
	{
		Estado_Lectura_IRS = IR_INACTIVO;
		Almacena_Luminosidad_Barras();
		FLAG_lectura_ir_finalizada = 1;

		//Actualizamos la posicion de la linea solo si hay linea (Con objetivo de que siga girando si se pierde)
		if (HayLinea())	
			Posicion_Linea = getPosicionLinea(Bias_Bifurcacion); 



		Resultado_PID_Direccion = Control_PID(Kp_direccion, Ki_direccion, Kd_direccion,Posicion_Linea,consigna_posicion,&Error_Acumulado_direccion,&Error_Anterior_direccion);
	}
}
void Escala_Luminosidad_Barras(void)
{
	int i;
	float m;
	float aux;
	for (i=0;i<24;i++)
	{
		m = IR_MAX/rango_dinamico_frontal[i];
		aux = (Luminosidad_Barra_Frontal[i]-Calibraciones_Frontal_Claro[i])*m;
		if (aux < 0) aux = 0;
		if (aux > 5000) aux = 5000;
		Luminosidad_Escalada_Frontal[i] = (uint16_t)aux;
	}

	for (i=0;i<16;i++)
	{
		m = IR_MAX/rango_dinamico_trasera[i];
		aux = (Luminosidad_Barra_Trasera[i]-Calibraciones_Trasera_Claro[i])*m;
		if (aux < 0) aux = 0;
		if (aux > 5000) aux = 5000;
		Luminosidad_Escalada_Trasera[i] = (uint16_t)aux;
	}

}
void Almacena_Luminosidad_Barras(void)
{
	Luminosidad_Barra_Frontal[0]  = Luminosidad_PortB[13];
	Luminosidad_Barra_Frontal[1]  = Luminosidad_PortE[1];
	Luminosidad_Barra_Frontal[2]  = Luminosidad_PortD[4];
	Luminosidad_Barra_Frontal[3]  = Luminosidad_PortD[5];
	Luminosidad_Barra_Frontal[4]  = Luminosidad_PortB[11];
	Luminosidad_Barra_Frontal[5]  = Luminosidad_PortD[7];
	Luminosidad_Barra_Frontal[6]  = Luminosidad_PortD[11];
	Luminosidad_Barra_Frontal[7]  = Luminosidad_PortE[10];
	Luminosidad_Barra_Frontal[8]  = Luminosidad_PortE[9];
	Luminosidad_Barra_Frontal[9]  = Luminosidad_PortD[1];
	Luminosidad_Barra_Frontal[10] = Luminosidad_PortD[8];
	Luminosidad_Barra_Frontal[11] = Luminosidad_PortE[8];
	Luminosidad_Barra_Frontal[12] = Luminosidad_PortE[15];
	Luminosidad_Barra_Frontal[13] = Luminosidad_PortE[7];
	Luminosidad_Barra_Frontal[14] = Luminosidad_PortE[14];
	Luminosidad_Barra_Frontal[15] = Luminosidad_PortD[0];
	Luminosidad_Barra_Frontal[16] = Luminosidad_PortE[13];
	Luminosidad_Barra_Frontal[17] = Luminosidad_PortD[15];
	Luminosidad_Barra_Frontal[18] = Luminosidad_PortE[12];
	Luminosidad_Barra_Frontal[19] = Luminosidad_PortD[14];
	Luminosidad_Barra_Frontal[20] = Luminosidad_PortD[9];
	Luminosidad_Barra_Frontal[21] = Luminosidad_PortE[11];
	Luminosidad_Barra_Frontal[22] = Luminosidad_PortB[10];
	Luminosidad_Barra_Frontal[23] = Luminosidad_PortD[10];

	Luminosidad_Barra_Trasera[0]   = Luminosidad_PortE[2];
	Luminosidad_Barra_Trasera[1]   = Luminosidad_PortE[3];
	Luminosidad_Barra_Trasera[2]   = Luminosidad_PortE[4];
	Luminosidad_Barra_Trasera[3]   = Luminosidad_PortE[5];
	Luminosidad_Barra_Trasera[4]   = Luminosidad_PortE[6];
	Luminosidad_Barra_Trasera[5]   = Luminosidad_PortC[1];
	Luminosidad_Barra_Trasera[6]   = Luminosidad_PortB[0];
	Luminosidad_Barra_Trasera[7]   = Luminosidad_PortC[3];
	Luminosidad_Barra_Trasera[8]   = Luminosidad_PortC[2];
	Luminosidad_Barra_Trasera[9]   = Luminosidad_PortC[4];
	Luminosidad_Barra_Trasera[10]  = Luminosidad_PortC[5];
	Luminosidad_Barra_Trasera[11]  = Luminosidad_PortB[9];
	Luminosidad_Barra_Trasera[12]  = Luminosidad_PortE[0];
	Luminosidad_Barra_Trasera[13]  = Luminosidad_PortB[8];
	Luminosidad_Barra_Trasera[14]  = Luminosidad_PortD[3];
	Luminosidad_Barra_Trasera[15]  = Luminosidad_PortB[1];

	Escala_Luminosidad_Barras();

	if (calibrando_barras) Calibra_Barras();
}

void Envia_Telemetria(void)
{
	int i,j;
	char *punt_char;
	//uint32_t aux_32bit;
	uint16_t aux_16bit;
	
	i=0;
	USART1_pila_TX[i++] = USART1_RESPUESTA_TELEMETRIA;
	USART1_pila_TX[i++] = 250;
	USART1_pila_TX[i++] = 0xFF & ~USART1_pila_TX[1];
	for (j=0;j<24;j++)
	{
		aux_16bit = (uint16_t)(Luminosidad_Barra_Frontal[j]);
		punt_char = (char *)&(aux_16bit);
		USART1_pila_TX[i++]   = *(punt_char++);
		USART1_pila_TX[i++] = *(punt_char++);
	}
	for (j=0;j<24;j++)
	{
		aux_16bit = (uint16_t)(Calibraciones_Frontal_Oscuro[j]);
		punt_char = (char *)&(aux_16bit);
		USART1_pila_TX[i++]   = *(punt_char++);
		USART1_pila_TX[i++] = *(punt_char++);
	}
	for (j=0;j<24;j++)
	{
		aux_16bit = (uint16_t)(Calibraciones_Frontal_Claro[j]);
		punt_char = (char *)&(aux_16bit);
		USART1_pila_TX[i++]   = *(punt_char++);
		USART1_pila_TX[i++] = *(punt_char++);
	}

	for (j=0;j<16;j++)
	{
		aux_16bit = (uint16_t)(Luminosidad_Barra_Trasera[j]);
		punt_char = (char *)&(aux_16bit);
		USART1_pila_TX[i++]   = *(punt_char++);
		USART1_pila_TX[i++] = *(punt_char++);
	}
	for (j=0;j<16;j++)
	{
		aux_16bit = (uint16_t)(Calibraciones_Trasera_Oscuro[j]);
		punt_char = (char *)&(aux_16bit);
		USART1_pila_TX[i++]   = *(punt_char++);
		USART1_pila_TX[i++] = *(punt_char++);
	}
	for (j=0;j<16;j++)
	{
		aux_16bit = (uint16_t)(Calibraciones_Trasera_Claro[j]);
		punt_char = (char *)&(aux_16bit);
		USART1_pila_TX[i++]   = *(punt_char++);
		USART1_pila_TX[i++] = *(punt_char++);
	}

	punt_char = (char *)&Posicion_Linea;
	USART1_pila_TX[i++] = *(punt_char++);
	USART1_pila_TX[i++] = *(punt_char++);
	USART1_pila_TX[i++] = *(punt_char++);
	USART1_pila_TX[i++] = *(punt_char++);

	punt_char = (char *)&Velocidad_metros_s;
	USART1_pila_TX[i++] = *(punt_char++);
	USART1_pila_TX[i++] = *(punt_char++); 
	USART1_pila_TX[i++] = *(punt_char++); 
	USART1_pila_TX[i++] = *(punt_char++);

	aux_16bit = 0xFFFF & TIM2->CCR1;
	punt_char = (char *)&aux_16bit;
	USART1_pila_TX[i++] = *(punt_char++);
	USART1_pila_TX[i++] = *(punt_char++); 

//	aux_32bit = PERIODO_MEDIDA_VELOCIDAD_MS;
//	punt_char = (char *)&aux_32bit;
//	USART1_pila_TX[i++] = *(punt_char++);
//	USART1_pila_TX[i++] = *(punt_char++); 
//	USART1_pila_TX[i++] = *(punt_char++); 
//	USART1_pila_TX[i++] = *(punt_char++);



	USART_ITConfig(USART1, USART_IT_TXE , ENABLE);
}

void Comienza_Lectura_Barras(void)
{
	uint16_t PortValue;
	GPIO_InitTypeDef GPIO_InitStructure;
	FLAG_Comienza_Lectura_Barras = 0;
	Estado_Lectura_IRS = IR_CARGANDO_BARRAS;
	
	/*Configuramos todos los pines asociados a la barra frontal como salida*/
	
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 

  GPIO_InitStructure.GPIO_Pin = PINES_BARRAS_PORTB;
  GPIO_Init(GPIOB, &GPIO_InitStructure);
	
  GPIO_InitStructure.GPIO_Pin = PINES_BARRAS_PORTC; 
  GPIO_Init(GPIOC, &GPIO_InitStructure); 

  GPIO_InitStructure.GPIO_Pin = PINES_BARRAS_PORTD; 
  GPIO_Init(GPIOD, &GPIO_InitStructure); 

	GPIO_InitStructure.GPIO_Pin = PINES_BARRAS_PORTE;
  GPIO_Init(GPIOE, &GPIO_InitStructure);

 /*Ponemos los pines asociados a las barras a nivel alto*/
	PortValue = GPIO_ReadOutputData(GPIOB);
	GPIO_Write(GPIOB,PortValue | PINES_BARRAS_PORTB);

	PortValue = GPIO_ReadOutputData(GPIOC);
	GPIO_Write(GPIOC,PortValue | PINES_BARRAS_PORTC);

	PortValue = GPIO_ReadOutputData(GPIOD);
	GPIO_Write(GPIOD,PortValue | PINES_BARRAS_PORTD);

	PortValue = GPIO_ReadOutputData(GPIOE);
	GPIO_Write(GPIOE,PortValue | PINES_BARRAS_PORTE);
	
	//Reseteamos TIM5. En el main veremos cuando sobrepasa IR_TIEMPO_CARGA para pasar a la siguiente fase de la medida de barras
	TIM5->CNT = 0;
}

/*Configuracion de las interrupciones*/
void NVIC_Configuration(void)
{
  NVIC_InitTypeDef NVIC_InitStructure;

  /* configuramos la interrupcion de la USART1 (Misma rutina para Recepcion y Transmision), asignando prioridades */
  NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn; //Canal IRQ de la USART1;
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; //Nivel de prioridad
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; //Nivel de sub-prioridad
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //Canal activo
  NVIC_Init(&NVIC_InitStructure);

	/* configuramos la interrupcion del timer 1 */
  NVIC_InitStructure.NVIC_IRQChannel = TIM1_UP_IRQn; //Canal IRQ del TIM1
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; //Nivel de prioridad
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; //Nivel de sub-prioridad
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //Canal activo
  NVIC_Init(&NVIC_InitStructure);


	/* configuramos la interrupcion del final de carrera de las puertas*/
  GPIO_EXTILineConfig(GPIO_PortSourceGPIOA, GPIO_PinSource8);

  EXTI_InitStructure.EXTI_Line = EXTI_Line8;
  EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
  EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;  
  EXTI_InitStructure.EXTI_LineCmd = ENABLE;
  EXTI_Init(&EXTI_InitStructure);

  NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn; 
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; 
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init(&NVIC_InitStructure);

//  /* configuramos la interrupcion del timer 3 */
//  NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn; //Canal IRQ del TIM5
//  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; //Nivel de prioridad
//  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; //Nivel de sub-prioridad
//  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //Canal activo
//  NVIC_Init(&NVIC_InitStructure);

//  /* configuramos la interrupcion del timer 4 */
//  NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
//  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
//  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
//  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
//  NVIC_Init(&NVIC_InitStructure);
}

void USART_Configuration(void)
{
  USART_InitStructure.USART_BaudRate = USART1_BAUDIOS;
  USART_InitStructure.USART_WordLength = USART_WordLength_8b;
  USART_InitStructure.USART_StopBits = USART_StopBits_1;
  USART_InitStructure.USART_Parity = USART_Parity_No;
  USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
  USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
  USART_Init(USART1,&USART_InitStructure);
  USART_Cmd(USART1, ENABLE);
}

void GPIO_Configuration(void)
{ 
	GPIO_InitTypeDef GPIO_InitStructure;

  /* Configure LED pin (PB5) as output push-pull */ 
  GPIO_InitStructure.GPIO_Pin = Pin_LED;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 
  GPIO_Init(Puerto_LED, &GPIO_InitStructure); 

  /* Configure the push_button pin (PB15) as input pull-up*/ 
  GPIO_StructInit(&GPIO_InitStructure);
  GPIO_InitStructure.GPIO_Pin = Pin_Boton; 
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; 
  GPIO_Init(Puerto_Boton, &GPIO_InitStructure);

	//Fines de carrera como input pull-up
  GPIO_StructInit(&GPIO_InitStructure);
  GPIO_InitStructure.GPIO_Pin = Pin_FC_Puertas; 
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; 
  GPIO_Init(Puerto_FC_Puertas, &GPIO_InitStructure);

  GPIO_StructInit(&GPIO_InitStructure);
  GPIO_InitStructure.GPIO_Pin = Pin_FC_Colision; 
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; 
  GPIO_Init(Puerto_FC_Colision, &GPIO_InitStructure);

  /* Configure USART1 Tx (PA9) as alternate function push-pull */ 
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; 
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; 
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; 
  GPIO_Init(GPIOA, &GPIO_InitStructure); 

  /* Configure USART1 Rx (PA10) as input floating */ 
  GPIO_StructInit(&GPIO_InitStructure);
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10; 
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 
  GPIO_Init(GPIOA, &GPIO_InitStructure); 

  /* Configure PWM output pins (PA0-PA2) as alternate function push-pull */ 
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_1 | GPIO_Pin_0; 
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; 
  GPIO_Init(GPIOA, &GPIO_InitStructure); 

	//Altavoz
  GPIO_InitStructure.GPIO_Pin = Pin_Altavoz;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 
  GPIO_Init(Puerto_Altavoz, &GPIO_InitStructure); 

	//ENTRADAS ENCODER:
 	/* Remapeamos la funcionalidad de TIM3 a las patas PC6, PC7, PC8, PC9 */
	GPIO_PinRemapConfig(GPIO_FullRemap_TIM3,ENABLE);
	/* Configuramos PC6 y PC7 como entradas flotantes, para el encoder*/
	GPIO_StructInit(&GPIO_InitStructure);
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; 
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 
  GPIO_Init(GPIOC, &GPIO_InitStructure);
	
  //ENTRADAS DE LA RADIO:
	//Entrada de direccion.
  GPIO_StructInit(&GPIO_InitStructure);
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12; 
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 
  GPIO_Init(GPIOD, &GPIO_InitStructure);
	
	//Entrada del switch
  GPIO_StructInit(&GPIO_InitStructure);
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13; 
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 
  GPIO_Init(GPIOD, &GPIO_InitStructure);
	
	//Entrada de aceleracion.
  GPIO_StructInit(&GPIO_InitStructure);
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6; 
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 
  GPIO_Init(GPIOB, &GPIO_InitStructure);
	
	//Entrada aux.
  GPIO_StructInit(&GPIO_InitStructure);
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7; 
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 
  GPIO_Init(GPIOB, &GPIO_InitStructure); 
}

void TIMER_Configuration(void)
{
	RCC_ClocksTypeDef RCC_ClocksStatus;
	RCC_GetClocksFreq(&RCC_ClocksStatus);  

  //   --------------------   TIMER 1 (1 MHz / 1us)  ------------------  
  //	 ----------	  Cuenta milisegundos en interrupcion ---------------
  //Inicializamos una estructura de configuracion del TIMER a valores por defecto
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);

  // Calculamos el valor del prescaler para una frecuencia del 1Mhz de cuenta
  TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t) (RCC_ClocksStatus.PCLK2_Frequency / 1000000) - 1;
  
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseStructure.TIM_Period = 1000; // ARR / Periodo
  
  /* Cargamos los registros con los valores de la estructura*/
  TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);

  /* TIM1 enable counter */
  TIM_Cmd(TIM1, ENABLE);


	//		--------------------   TIMER 2 (PWM outputs)  ------------------  
  /*Inicializamos una estructura de configuracion del TIMER a valores por defecto*/
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
  
  /* Introducimos los valores en la estructura*/
	TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t) (RCC_ClocksStatus.PCLK1_Frequency / 3250000) - 1;
	//TIM_TimeBaseStructure.TIM_Prescaler = 16-1; 
	TIM_TimeBaseStructure.TIM_Period = 0xFFFF; //Rebosa a 50Hz
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  /* Cargamos los registros con los valores de la estructura*/
  TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);									 									 
  
	
	/*Inicializamos una estructura de configuracion del Output compare a valores por defecto*/
  TIM_OCStructInit(&TIM_OCInitStructure);
  /* PWM1 Mode configuration: Channels 1-4 (PA0-PA3) */
  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  TIM_OCInitStructure.TIM_Pulse = CENTRO_ACC;
  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
  TIM_OC1Init(TIM2, &TIM_OCInitStructure);
	TIM_OC2Init(TIM2, &TIM_OCInitStructure);
	TIM_OC3Init(TIM2, &TIM_OCInitStructure);
	TIM_OC4Init(TIM2, &TIM_OCInitStructure);
  TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable);
	TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable);
	TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
	TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);
	TIM2->CCR1=CENTRO_DIR;
	TIM2->CCR2=CENTRO_ACC;
	TIM2->CCR3=CENTRO_DIR;
	//TIM2->CCR4=26666 ;
  /* TIM2 enable counter */
  TIM_Cmd(TIM2, ENABLE);

//--------------------   TIMER 3 (encoder input)  ------------------------  

	TIM_EncoderInterfaceConfig(TIM3,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
	TIM_Cmd(TIM3, ENABLE);


//--------------------   TIMER 4 (PWM input direccion)  ------------------  
  
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
	TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t) (RCC_ClocksStatus.PCLK1_Frequency / 3250000) - 1;
  TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);

	//Este trozo configura la senal TI1FP1 para ser los flancos de subida de la entrada del canal 1 sin filtrar, sin prescala.
  TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
  TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
  TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
  TIM_ICInitStructure.TIM_ICPrescaler = 0;
  TIM_ICInitStructure.TIM_ICFilter = 0x0;
  TIM_PWMIConfig(TIM4, &TIM_ICInitStructure);

  /* Select the TIM4 Input Trigger: TI1FP1 */
  TIM_SelectInputTrigger(TIM4, TIM_TS_TI1FP1);
  /* Select the slave Mode: Reset Mode */
  TIM_SelectSlaveMode(TIM4, TIM_SlaveMode_Reset);
  /* Enable the Master/Slave Mode */
  TIM_SelectMasterSlaveMode(TIM4, TIM_MasterSlaveMode_Enable);
  /* TIM enable counter */
  TIM_Cmd(TIM4, ENABLE);


  //   --------------------   TIMER 5 (1 MHz / 1us)  ------------------  
  //	 --------	  Utilizado para contar tiempos de las barras ---------

  //Inicializamos una estructura de configuracion del TIMER a valores por defecto
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);

  // Calculamos el valor del prescaler para una frecuencia del 0.1Mhz de cuenta
  TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t) (RCC_ClocksStatus.PCLK1_Frequency / 1000000) - 1;
  
  /* Configuraremos el periodo para que el contador se reinicie cada milisegundo
                  esto se produce cuando el contador llega al valor TIM5_ARR.
          Dicho de otra forma: TIM5_CounterClock / Fdeseada - 1 = TIM5_ARR
          100 Khz / 1Khz - 1 = 99
  */
  
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  
  /* Cargamos los registros con los valores de la estructura*/
  TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);

  /* TIM2 enable counter */
  TIM_Cmd(TIM5, ENABLE);
}


#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t* file, uint32_t line)
{ 
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */

  /* Infinite loop */
  while (1)
  {
  }
}

#endif


